# Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
# Use of this source code is governed by a BSD-style license that can be
# found in the LICENSE file.

import copy
import math
import time

from numpy import clip
from position import Position
from profile import Profile
from touchbotcomm import TouchbotComm


class Touchbot:
    """ High level Touchbot control class

    This class hides the internals of communicating with the robot and
    offers high level robot control commands.
    """

    # Network connection parameters for the robot
    TOUCHBOT_IP_ADDR = '192.168.0.1'
    TOUCHBOT_PORT = 10100

    # How high above to approach from to be safe
    SAFETY_CLEARANCE = 220

    # Useful speeds for movement
    SPEED_VERY_SLOW = 1.0
    SPEED_SLOW = 5.0
    SPEED_MEDIUM = 10.0
    SPEED_FAST = 15.0
    SPEED_VERY_FAST = 20.0
    MAX_SPEED = 100.0

    # Limits on how far the robot hand can move the fingers in and out
    MIN_FINGER_DISTANCE = 15
    MAX_FINGER_DISTANCE = 140

    # How long it takes for the fingertip pistons to extend (in seconds)
    MINIMUM_FINGER_EXTENSION_TIME = 0.09

    # Setting for the accuracy (InRange) value of a profile
    BLEND_MOVEMENTS = -1
    ALWAYS_STOP = 0
    MAX_ACCURACY = 1

    # Settings for how to interpolate points (straight or curved)
    STRAIGHT_INTERPOLATION = -1
    JOINT_INTERPOLATION = 0

    # Indicies for adressing specific axes of the robot's arm
    AXIS_Z = 1
    AXIS_SHOULDER = 2
    AXIS_ELBOW = 3
    AXIS_WRIST = 4
    AXIS_HAND = 5
    ALL_AXES = [AXIS_Z, AXIS_SHOULDER, AXIS_ELBOW, AXIS_WRIST, AXIS_HAND]

    # GPIO addresses for the robot's fingers
    FINGER_BR = 16
    FINGER_FR = 15
    FINGER_FL = 14
    FINGER_BL = 13
    ALL_FINGERS = [FINGER_FR, FINGER_FL, FINGER_BL, FINGER_BR]

    # GPIO addresses for the nest close and open valves
    NEST_CLOSE_VALVE = 800013
    NEST_OPEN_VALVE = 800015

    # GPIO address for the fingertip detection sensor
    FINGERTIP_DETECT = 810005
    FINGERTIP_DETECTION_REPITITIONS = 10

    # Paths to pre-recorded Positions hovering above the nest
    NEST_POSITION_FILES = ['nest/size0.p', 'nest/size1.p',
                           'nest/size2.p', 'nest/size3.p']

    # Paths to pre-recorded Positions hovering above the fingertip detector
    FINGERTIP_DETECT_POSITION_FILES = ['detector/fingerfr.p',
                                       'detector/fingerfl.p',
                                       'detector/fingerbl.p',
                                       'detector/fingerbr.p']

    # It appears that the units are not exactly mm, but are scaled slightly
    # By scaling distances by this value the accuracy goes up dramatically
    DISTANCE_SCALING_FACTOR = 0.75

    # The range of legal relative coordinates.
    RELATIVE_COORDINATE_RANGE = (-0.05, 1.05)

    # The range of non-binding values for ax_4
    AX_4_MIN = 1
    AX_4_MAX = 359

    def __init__(self, ip_address=TOUCHBOT_IP_ADDR, port=TOUCHBOT_PORT):
        self.speed_stack = []
        self.comm = TouchbotComm(ip_address, port)

        self.nest_positions = [Position.FromPickledFile(filename)
                               for filename in Touchbot.NEST_POSITION_FILES]
        if not all(self.nest_positions):
            print 'WARNING: Some nest positions loaded incorrectly'

        self.fingertip_detect_positions = [Position.FromPickledFile(filename)
             for filename in Touchbot.FINGERTIP_DETECT_POSITION_FILES]
        if not all(self.fingertip_detect_positions):
            print 'WARNING: Some fingertip detector position loaded incorrectly'

        self.SetFingerStates([0, 0, 0, 0])
        self.OpenNest()

    def CalibratePosition(self):
        """ Make the robot go limp and record its position after the operator
        moves it into the desired configuration.
        Returns a Position object of the recorded posisition.
        """
        successful = True
        response = self.FreeAxes()
        if not response or response[0] != 0:
            print 'There was an error entering free mode.'
            return None

        print 'Move the robot CAREFULLY to where you want it.'
        raw_input('Press "enter" to record that position.')

        response = self.UnFreeAxes()
        if not response or response[0] != 0:
            print 'There was an error leaving free mode.'
            return None

        return self.GetCurrentPosition()

    def SetSpeed(self, speed):
        prof = self.GetCurrentProfile()
        prof.speed = speed
        self.SetProfileData(prof)

    def GetSpeed(self):
        prof = self.GetCurrentProfile()
        return prof.speed

    def FreeAxes(self):
        """ Set all axis into free mode, ie: make them go limp. """
        return self.comm.SendCmd('freemode', 0)

    def UnFreeAxes(self):
        """ Return all axes to computer control. """
        return self.comm.SendCmd('freemode', -1)

    def GetCurrentPosition(self):
        """ Record the current position as a Position object. """
        response = self.comm.SendCmd('where')
        if not response:
            return None
        error_code, data = response
        if error_code != 0:
            return None
        return Position.FromTouchbotResponse(data)

    def GetCurrentProfile(self):
        """ Get a copy of the current movement profile. """
        error_code, data = self.comm.SendCmd('profile')
        return Profile.FromTouchbotResponse(data)

    def SetProfileData(self, profile):
        """ Replace the robot's current profile with this one. """
        return self.comm.SendCmd('profile', str(profile))

    def GetGPIO(self, gpio_num):
        """ Get the value of a GPIO input from the robot. """
        error_code, raw_data = self.comm.SendCmd('sig', gpio_num)
        if error_code == 0:
            return int(raw_data.split()[1])
        else:
            return None

    def SetGPIO(self, gpio_num, value):
        """ Set the value of a GPIO output on the robot. """
        return self.comm.SendCmd('sig', gpio_num, value)

    def SetAngles(self, pos, blocking=True):
        """ Move the robot to pos by the raw joint angles. """
        response = self.comm.SendCmd('movej', pos.ax_1, pos.ax_2, pos.ax_3,
                                     pos.ax_4, pos.ax_5)
        if blocking:
            self.Wait()
        return response

    def SetCartesian(self, pos, finger_distance=None, blocking=True):
        """ Move the robot to pos by Cartesian coordinates. """
        # If a finger spacing has been specified add that to the movement
        # command.  The finger spacing motors were added as an additional axis
        # so are not controlled by the 'movec' command by default.  They must
        # be specified before with the 'moveExtraAxis' command, and then are
        # moved when the robot receives the next 'movec.'
        if finger_distance:
            finger_distance = clip(finger_distance,
                                   Touchbot.MIN_FINGER_DISTANCE,
                                   Touchbot.MAX_FINGER_DISTANCE)
            adjusted_dist = finger_distance * Touchbot.DISTANCE_SCALING_FACTOR
            self.comm.SendCmd('moveExtraAxis', adjusted_dist)

        response = self.comm.SendCmd('movec', pos.x, pos.y, pos.z,
                                     pos.yaw, pos.pitch, pos.roll)
        if blocking:
            self.Wait()
        return response

    def Wait(self):
        return self.comm.SendCmd('waitForEom')

    def AddSafetyClearance(self, pos):
        new_pos = copy.deepcopy(pos)
        new_pos.ax_1 = Touchbot.SAFETY_CLEARANCE
        new_pos.z = Touchbot.SAFETY_CLEARANCE
        return new_pos

    def ManipulateFingertips(self, fingers, size, should_keep_fingertips):
        """ Either attach or detach the selected fingers. """
        if size < 0 or size > len(self.nest_positions) or len(fingers) != 4:
            return False

        curr_pos = self.GetCurrentPosition()
        above_curr_pos = self.AddSafetyClearance(curr_pos)

        nest_pos = self.nest_positions[size]
        above_nest_pos = self.AddSafetyClearance(nest_pos)

        response = self.SetAngles(above_curr_pos)
        self.OpenNest()
        response = self.SetAngles(above_nest_pos)
        if not response or response[0] != 0:
            print 'Error getting into position for size %d fingertips' % size
            return False
        self.SetAngles(nest_pos)

        self.SetFingerStates(fingers)

        if not should_keep_fingertips:
            time.sleep(1)
            self.CloseNest()

        time.sleep(1)
        self.SetFingerStates([0, 0, 0, 0])
        self.SetAngles(above_nest_pos)
        self.OpenNest()
        return True

    def DetectFingertips(self):
        """ Check the state of the fingertips -- see if they're connected. """
        states = []
        for pos in self.fingertip_detect_positions:
            self.SetAngles(pos)
            readings = []
            for i in range(Touchbot.FINGERTIP_DETECTION_REPITITIONS):
                    time.sleep(0.1)
                    readings.append(self.GetGPIO(Touchbot.FINGERTIP_DETECT))
            num_present = sum([1 for reading in readings if reading != 0])
            states.append(num_present >=
                          (Touchbot.FINGERTIP_DETECTION_REPITITIONS / 2.0))
            print 'Finger readings %s -> %d' % (str(readings), states[-1])
        return states

    def SetFingerStates(self, states):
        """ Control which fingers are extended and which are retracted. """
        for finger, state in zip(Touchbot.ALL_FINGERS, states):
            self.SetGPIO(finger, state)

    def FingerStates(self):
        """ See which fingers are extended and which are retracted. """
        return [self.GetGPIO(finger) for finger in Touchbot.ALL_FINGERS]

    def OpenNest(self):
        """ Open the nest to allow fingertips to pass in and out. """
        self.SetGPIO(Touchbot.NEST_CLOSE_VALVE, 0)
        self.SetGPIO(Touchbot.NEST_OPEN_VALVE, 1)

    def CloseNest(self):
        """ Close the nest to keep fingertips from leaving. """
        self.SetGPIO(Touchbot.NEST_CLOSE_VALVE, 1)
        self.SetGPIO(Touchbot.NEST_OPEN_VALVE, 0)

    def IsLegalSpeed(self, speed):
        """ Is a speed an acceptable value for this robot """
        return speed > 0.0 and speed <= Touchbot.MAX_SPEED

    def IsLegalRelativeCoordinate(self, coords):
        """ Are the x and y values between 0 and 1 """
        if len(coords) != 2:
            return False
        x, y = coords
        min_val, max_val = Touchbot.RELATIVE_COORDINATE_RANGE
        return min_val <= x <= max_val and min_val <= y <= max_val

    def IsLegalFingerList(self, fingers):
        """ Check that a finger selection list is legal """
        return (len(fingers) == len(Touchbot.ALL_FINGERS) and
                all([bool(f in [0, 1]) for f in fingers]))

    def CenterIfSingleFinger(self, fingers, abs_pos, finger_distance):
        """ Offset them if only 1 finger is down (you're on your own for >1
        finger) This way the *finger* itself will be centered at the point,
        not the point where all the fingers meet.
        """
        if len(fingers) == 4 and sum(fingers) == 1:
            # Find which finger is down and how many degrees off center it is
            finger_index = fingers.index(1)
            finger_angle_offset = (90 * finger_index) - 45

            # Shift the x and y accordingly for both points in the line
            angle = math.radians(abs_pos.yaw + finger_angle_offset)
            abs_pos.x += math.cos(angle) * finger_distance / 2.0
            abs_pos.y += math.sin(angle) * finger_distance / 2.0

        return abs_pos

    def NonBindingYawRange(self, pos=None, safety_buffer=0):
        """ Compute the range of legal yaw values for a given arm configuration
        If no position is given, the robot's current position will be used.
        """
        self.PushSpeed(Touchbot.MAX_SPEED)

        pos = self.GetCurrentPosition() if not pos else pos
        pos.ax_4 = self.AX_4_MIN
        self.SetAngles(pos)
        pos = self.GetCurrentPosition()
        yaw_min = pos.yaw

        pos.ax_4 = self.AX_4_MAX
        self.SetAngles(pos)
        pos = self.GetCurrentPosition()
        yaw_max = pos.yaw

        if yaw_max - yaw_min < 360:
            yaw_max += 360

        self.PopSpeed()
        return (yaw_min + safety_buffer, yaw_max - safety_buffer)

    def PushSpeed(self, new_speed):
        self.speed_stack.append(self.GetSpeed())
        self.SetSpeed(new_speed)

    def PopSpeed(self):
        if len(self.speed_stack) >= 1:
            self.SetSpeed(self.speed_stack.pop())
